#include "cvGabor.h"
#include "../CommonUsed/TMatrix.h"

CvMat* cvConv(const CvMat* src, const CvMat* kernel)
{
	CvMat* dst = cvCreateMat(src->rows,src->cols,CV_32F);
	cvFilter2D(src,dst,kernel);
	return dst;
}

void cvGaborConv(const CvArr* src, CvMat* dst1, const CGaborFilter* filter)
{
	CvMat* dst2 = cvCreateMat(dst1->rows,dst1->cols,CV_32F);
	cvFilter2D(src,dst1,filter->GetFilterMatIm());
	cvFilter2D(src,dst2,filter->GetFilterMatRe());
	/*cvMul(dst1,dst1,dst1);
	cvMul(dst2,dst2,dst2);
	cvAdd(dst1,dst2,dst2);
	for (int i=0;i<dst1->rows;i++)
	{
		for (int j=0;j<dst1->cols;j++)
		{
			cvmSet(dst1,i,j,cvSqrt(cvmGet(dst2,i,j)));
		}
	}*/
	cvCartToPolar(dst1,dst2,dst1);
	cvReleaseMat(&dst2);
}

CGaborFilter* CGaborFilter::GenerateGaborFilter( int nScale, int nOrientation, int m_ScaleTotal, int m_OrientationTotal)
{
	double  dScale,  tcos, tsin;
	double u0, z, Uvar, Vvar;
	double Xvar, Yvar, X, Y, G0,G;
	int x, y;


	//Compute the scale factor
	dScale = pow(UH/UL, 1.0/(m_ScaleTotal-1));

	tcos = cos(MATHPI/m_OrientationTotal * nOrientation);
	tsin = sin(MATHPI/m_OrientationTotal * nOrientation);

	u0 = UH/pow(dScale, (double) m_ScaleTotal-nScale);

	Uvar = (dScale-1.0)*u0/((dScale+1.0)*sqrt(2.0*log(2.0)));

	z = -2.0*log(2.0)*(Uvar*Uvar)/u0;
	Vvar = tan(MATHPI/(2*m_OrientationTotal))*(u0+z)/sqrt(2.0*log(2.0)-z*z/(Uvar*Uvar));

	Xvar = 1.0/(2.0*MATHPI*Uvar);
	Yvar = 1.0/(2.0*MATHPI*Vvar);

	G0=1.0/(2.0*MATHPI*Xvar*Yvar) * u0/UH;

	int nWindowSize=2*NSIZE+1;

	FAST::TMatrix<double> GaborFilterIm, GaborFilterRe;

	GaborFilterIm.NewSize(nWindowSize, nWindowSize);
	GaborFilterRe.NewSize(nWindowSize, nWindowSize);

	for (x=0;x<nWindowSize;x++) {
		for (y=0;y<nWindowSize;y++) {
			X = (double) (x-NSIZE)*tcos+ (double) (y-NSIZE)*tsin;
			Y = (double) -(x-NSIZE)*tsin+ (double) (y-NSIZE)*tcos;
			G = G0 * exp(-0.5*((X*X)/(Xvar*Xvar)+(Y*Y)/(Yvar*Yvar)));
			GaborFilterRe(x,y) =( G*cos(2.0*MATHPI*u0*X));
			GaborFilterIm(x,y) =( G*sin(2.0*MATHPI*u0*X));
		}
	}

	/* if flag = 1, then remove the DC from the filter */
	/*if (removeDC == true) {
		double 	m = 0;
		for (x=0;x<nWindowSize;x++)
			for (y=0;y<nWindowSize;y++)
				m += GaborFilterRe(x,y);

		m /= pow((double) nWindowSize, 2.0);

		for (x=0;x<nWindowSize;x++)
			for (y=0;y<nWindowSize;y++)
				GaborFilterRe(x,y) -=(GaborReal) m;
	}	*/

	//Discrete Gaborfilter

	FAST::TMatrix<int> GaborFilterIm1, GaborFilterRe1;
	GaborFilterRe1.NewSize(nWindowSize, nWindowSize);
	GaborFilterIm1.NewSize(nWindowSize, nWindowSize);

	double elMin, elMax,dStep;

	elMin = TAbs(GaborFilterRe.Min());
	elMax =  TAbs(GaborFilterRe.Max());

	dStep = elMax>elMin ? elMax : elMin;
	dStep/=GABOR_DISCRETE_LEVEL-1;

	GaborFilterRe1 = (TIntMat)(GaborFilterRe/dStep+0.5f);


	elMin = TAbs(GaborFilterIm.Min());
	elMax =  TAbs(GaborFilterIm.Max());

	dStep = elMax>elMin ? elMax : elMin;
	dStep/=GABOR_DISCRETE_LEVEL-1;

	GaborFilterIm1 = (TIntMat)(GaborFilterIm/dStep+0.5f);

	CGaborFilter* filter = new CGaborFilter();

	for (x=0;x<nWindowSize;x++) {
		for (y=0;y<nWindowSize;y++) {
			double re1 = GaborFilterRe1(x,y);
			double im1 = GaborFilterIm1(x,y);
			cvmSet(filter->m_GaborFilterRe,x,y,re1);
			cvmSet(filter->m_GaborFilterIm,x,y,im1);
		}
	}

	return filter;
}